package Model.SurroundModel;



import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.List;

public class Ship {
    // 以下为一阶包围所用参数
    public  ArrayList<Ship> ships = new ArrayList<Ship>(); //船队
    public static double PI = Math.PI;
    public int num; //船只编号
    public double v; //前进速度
    public double u; //角速度
    public double θ; //智能体和目标中心的连线与x的夹角
    public double l; //半径
    public double ρ; //当前小船对目标距离目标平均中心的最大估计值
    public double φx = 0;
    public double φy = 0;//表示目标平均中心动态估计器的内部状态
    public double x; //船的x坐标
    public double y; //船的y坐标 x,y用于y（t）
    public double x0; //船的x坐标
    public double y0; //船的y坐标 x0，y0用于计算r（t）
    public double xx; //船的x坐标
    public double yy; //船的y坐标 xx,yy用于计算p（t）p(t)是目标中心的位置
    public double ux; //船的x坐标
    public double uy; //船的y坐标 xx,yy用于计算p（t）p(t)是目标中心的位置
    public double el;
    public double eθ;


    // 以下为阵列变换所需参数
    public double x1;
    public double y1;//预期位置
    public boolean isScan; //是否经过碰撞检测扫描
    public boolean isFinish = false;//是否移动结束
    public ArrayList<Integer> Index = new ArrayList<Integer>();//路径交叉的船对应的索引
    public ArrayList<Double> history_x = new ArrayList<Double>();
    public ArrayList<Double> history_y = new ArrayList<Double>();//存储小船的移动路径点
    public LinkedHashMap<Integer, HashMap<String, Double>> dis = new LinkedHashMap<>();

    public Ship() {
    }


    public void setV(double v) {
        this.v = v;
    }

    public void setXY(double x1, double y1) {
        this.x = x1;
        this.y = y1;
    }

    public void storeXY() {

        history_x.add(this.x);
        history_y.add(this.y);
    }

    public void storeIndex(int a) {
        Index.add(a);
    }

    //计算欧氏距离
    public  double distance(double x1, double y1, double x2, double y2) {
        return Math.sqrt(Math.pow((x1 - x2), 2) + Math.pow((y1 - y2), 2));
    }



    public Ship(int num, double l, double θ, double ρ) {
        this.l = l;
        this.θ = θ;
        this.num = num;
        this.ρ = ρ;
    }

    public Ship(int num, double x, double y) {
        this.x = x;
        this.y = y;
        this.num = num;
    }



    //初始化智能体的信息
    public  void initShip() {
        this.ships.clear();
        this.ships.add(new Ship(1, 4, PI / 6.0, 5));
        this.ships.add(new Ship(2, 7, 2 * PI / 3.0, 5));
        this.ships.add(new Ship(3, 8, 7 * PI / 6.0, 5));
        this.ships.add(new Ship(4, 6, 5 * PI / 3.0, 5));
    }

    //首先获取初始时智能体的位置
    public  void setxy() {
        for (Ship ship : this.ships) {
            ship.x0 = Target.x0 + ship.l * Math.cos(ship.θ);
            ship.y0 = Target.y0 + ship.l * Math.sin(ship.θ);
        }
    }

    //初始化智能体位置
    public  void setPosition(List<Ship> shipList) {
        this.ships.clear();
        for (Ship item : shipList) this.ships.add(item);
//        this.ships.add(new Ship(1, -5, 9));
//        this.ships.add(new Ship(2, 3, 4));
//        this.ships.add(new Ship(3, 7, 6));
//        this.ships.add(new Ship(4, 9, 4));
    }


    //  接下来进行p（t）和 φ（t） 的计算
    public void setP(double t, Target Target) {
        for (Ship ship : this.ships) {
            ship.xx = Target.targets.get(ship.num - 1).x + ship.φx;
            ship.yy = Target.targets.get(ship.num - 1).y + ship.φy;
            BigDecimal B = new BigDecimal(ship.xx);
            ship.xx = B.setScale(3, RoundingMode.HALF_UP).doubleValue();
            BigDecimal C = new BigDecimal(ship.yy);
            ship.yy = C.setScale(3, RoundingMode.HALF_UP).doubleValue();
            Target.targets.get(ship.num - 1).xx = ship.xx;
            Target.targets.get(ship.num - 1).yy = ship.yy;
        }
    }

    public void setφ(double t) {
        double φ1 = 0;
        double φ2 = 0;
        for (Ship ship : this.ships) {
            φ1 = 0;
            φ2 = 0;
            if (ship.num == 1) {
                φ1 = φ1 + Math.signum(ships.get(ship.num).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num).yy - ship.yy);
            } else if (ship.num == 2) {
                φ1 = φ1 + Math.signum(ships.get(ship.num - 2).xx - ship.xx) + Math.signum(ships.get(ship.num).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num - 2).yy - ship.yy) + Math.signum(ships.get(ship.num).yy - ship.yy);
            } else if (ship.num == 3) {
                φ1 = φ1 + Math.signum(ships.get(ship.num - 2).xx - ship.xx) + Math.signum(ships.get(ship.num).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num - 2).yy - ship.yy) + Math.signum(ships.get(ship.num).yy - ship.yy);
            } else {
                φ1 = φ1 + Math.signum(ships.get(ship.num - 2).xx - ship.xx);
                φ2 = φ2 + Math.signum(ships.get(ship.num - 2).yy - ship.yy);
            }
            φ1 = Parameter.alpha * t * φ1;
            φ2 = Parameter.alpha * t * φ2;
            ship.φx = φ1 + ship.φx;
            ship.φy = φ2 + ship.φy;
        }
    }

    //接下来计算ρ(t),l（t）和θ（t）
    public  void setρ(double t,Target Target) {
        double d = 0;
        for (Ship ship : ships) {
            d = Target.targets.get(ship.num - 1).dmax(Target);
            //System.out.println(d);
            ship.ρ = ship.ρ - Math.signum(ship.ρ - d) * Parameter.k1 * t;
            //System.out.println("num:" + ship.num + " ρ：" + ship.ρ);
        }
    }

    public  void setl(double t) {
        for (Ship ship : ships) {
            ship.l = ship.l - Math.signum(ship.l - Parameter.k * ship.ρ) * Parameter.k2 * t;
            BigDecimal C = new BigDecimal(ship.l);
            ship.l = C.setScale(4, RoundingMode.HALF_UP).doubleValue();
            ship.v = -Math.signum(ship.l - Parameter.k * ship.ρ) * Parameter.k2;
        }
    }

    public  void setθ(double t) {
        for (Ship ship : ships) {
            int i = ship.num - 1;
            double θ1 = 0;
            for (Ship shipx : ships) {
                θ1 = Parameter.A[i][shipx.num - 1] * (ship.θ - shipx.θ - 2 * Math.PI * (shipx.num - ship.num) / Parameter.N) + θ1;
            }
            ship.θ = ship.θ + θ1 * t;
            ship.u = θ1;
        }
    }

    //接下来计算r（t）
    public  void setr(double t) {
        for (Ship ship : ships) {
            ship.x0 = ship.v * Math.cos(ship.θ) * t - ship.u * ship.l * Math.sin(ship.θ) * t + ship.x0;
            ship.y0 = ship.v * Math.sin(ship.θ) * t + ship.u * ship.l * Math.cos(ship.θ) * t + ship.y0;
        }
    }

    //接下来计算y(t)
    public  void sety(double t) {
        for (Ship ship : ships) {
            ship.x = ship.xx + ship.l * Math.cos(ship.θ);
            ship.y = ship.yy + ship.l * Math.sin(ship.θ);
        }
    }

    //圆形包围控制器
    public  void circularCotroller(double t,Target Target) {
        setρ(t,Target); //计算智能体对目标距离目标平均中心的最大距离估计值
        setl(t); //计算智能体的半径变化
        setθ(t); //计算智能体的角度变化
    }

    //目标队列包围控制器
    public void formationController(double t, Target Target) {
        double x = 0;
        double y = 0;
        int i = 0;
        int j = 0;

        setφ(t);
        setP(t,Target);

        for (Ship ship : ships) {
            i = ship.num - 1;
            ship.x0 = -FormationParameter.k0 * (ship.x - ship.xx - FormationParameter.k1 * (Target.targets.get(i).x - ship.xx));
            ship.y0 = -FormationParameter.k0 * (ship.y - ship.yy - FormationParameter.k1 * (Target.targets.get(i).y - ship.yy));
            //System.out.println("ship.num:" + ship.num + "  ship.ux:" + ship.ux + "  ship.uy:" + ship.uy);
            for (Ship shipx : ships) {
                j = ship.num - 1;
                x += FormationParameter.A[i][j] * ((shipx.x - shipx.xx - FormationParameter.k1 * (Target.targets.get(j).x - shipx.xx))
                        - (ship.x - ship.xx - FormationParameter.k1 * (Target.targets.get(i).x - ship.xx)));
                y += FormationParameter.A[i][j] * ((shipx.y - shipx.yy - FormationParameter.k1 * (Target.targets.get(j).y - shipx.yy))
                        - (ship.y - ship.yy - FormationParameter.k1 * (Target.targets.get(i).y - ship.yy)));

            }
            ship.ux = ship.x0 + x;
            ship.uy = ship.y0 + y;
            //System.out.println("ship.num:"+ship.num+ "  ship.ux:" + ship.ux + "  ship.uy:" + ship.uy);
            x = 0;
            y = 0;
            ship.x += ship.ux * t;
            ship.y += ship.uy * t;
        }

    }

    //判断停止的函数
//    public static boolean circularFinish(double t) {
//        double max = 0;
//        double e = 0;
//        int jud = 4;
//        for (int i = 0; i < Parameter.N; i++) {
//            if (distance(Target.targets.get(i).x, Target.targets.get(i).y, Target.x0, Target.y0) > max)
//                max = distance(Target.targets.get(i).x, Target.targets.get(i).y, Target.x0, Target.y0);
//        }
//        for (Ship ship : ships) {
//            ship.el = distance(ship.x, ship.y, Target.x0, Target.y0) - Parameter.k * max;
//            ship.eθ = 1;
//            for (Ship shipx : ships) {
//                e = ship.θ - shipx.θ - 2 * Math.PI * (shipx.num - ship.num) / Parameter.N;
//                if (e == 0)
//                    jud--;
//            }
//            if (jud == 0)
//                ship.eθ = 0;
//            jud = 4;
//        }
//        for (Ship ship : ships) {
//            if (Math.abs(ship.el) == 0 && Math.abs(ship.eθ) == 0) {
//                jud--;
//            }
//        }
//        if (jud <= 0) {
//            return true;
//        } else
//            return false;
//    }

    //判断停止的函数
    public  boolean circularFinish(double t,Ship Ship,Target Target) {
        double max = 0;
        double e = 0;
        int jud = 4;
        for (int i = 0; i < Parameter.N; i++) {
            if (distance(Target.targets.get(i).x, Target.targets.get(i).y, Ship.ships.get(i).xx, Ship.ships.get(i).yy) > max)
                max = distance(Target.targets.get(i).x, Target.targets.get(i).y, Ship.ships.get(i).xx, Ship.ships.get(i).yy);
        }
        for (Ship ship : ships) {
            ship.el = distance(ship.x, ship.y, ship.xx,ship.yy) - Parameter.k * max;
            ship.eθ = 1;
            for (Ship shipx : ships) {
                e = ship.θ - shipx.θ - 2 * Math.PI * (shipx.num - ship.num) / Parameter.N;
                if (e == 0)
                    jud--;
            }
            if (jud == 0)
                ship.eθ = 0;
            jud = 4;
        }
        for (Ship ship : ships) {
            if (Math.abs(ship.el) >= 0.01 && Math.abs(ship.eθ) == 0) {
                jud--;
            }
        }
        if (jud <= 0) {
            return true;
        } else
            return false;
    }

    //判断目标队列停止的函数
    public boolean formationFinish(double t, Target Target) {
        double e = 0;
        int i = 0;
        int jud = 4;
        double x1 = 0;
        double x2 = 0;
        double y1 = 0;
        double y2 = 0;
        Target.setCenter();

        for (Ship ship : ships) {
            i = ship.num - 1;
            x1 = ship.x - Target.x0;
            y1 = ship.y - Target.y0;
            x2 = FormationParameter.k * (Target.targets.get(i).x - Target.x0);
            y2 = FormationParameter.k * (Target.targets.get(i).y - Target.y0);
            e = distance(x1, y1, x2, y2);
            if (Math.abs(e) <= 1.5)
                jud--;
        }
        if (jud <= 0) {
            return true;
        } else {
            return false;
        }
    }

}
